/***************************************************************************
*   Copyright (C) 2010-2011 by swkyer <swkyer@gmail.com>                  *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License as published by  *
*   the Free Software Foundation; either version 2 of the License, or     *
*   (at your option) any later version.                                   *
*                                                                         *
*   This program is distributed in the hope that it will be useful,       *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU General Public License for more details.                          *
*                                                                         *
*   You should have received a copy of the GNU General Public License     *
*   along with this program; if not, write to the                         *
*   Free Software Foundation, Inc.,                                       *
*   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
***************************************************************************/
#include "stdafx.h"
#include "mips32opc.h"
#include "JlinkMIPS.h"
#include "Memory.h"
#include "CpuInfo.h"
#include "BreakPoint.h"


typedef struct _mips32_hwbp_reg_s
{
    ubase_t IBA;
    ubase_t IBM;
    ubase_t IBASID;
    ubase_t IBC;
} mips32_hwbp_reg_t;

typedef struct _mips32_hwwp_reg_s
{
    ubase_t DBA;
    ubase_t DBM;
    ubase_t DBASID;
    ubase_t DBC;
    ubase_t DBV;
    ubase_t DBVHI;
} mips32_hwwp_reg_t;



int mips32_bpt_init(mips32_bpt_info_t *pbpyinfo, void *pmips32)
{
    pbpyinfo->pmips32 = pmips32;
    pbpyinfo->bps = NULL;

    return 0;
}

int mips32_bpt_exit(mips32_bpt_info_t *pbpyinfo)
{
    mips32_bpt_t *bptmp;

    while (pbpyinfo->bps != NULL)
    {
        bptmp = pbpyinfo->bps->next;
        free(pbpyinfo->bps);
        pbpyinfo->bps = bptmp;
    }
    pbpyinfo->bps = NULL;
    pbpyinfo->pmips32 = NULL;
    return 0;
}

static int bps_get_swbp_count(mips32_bpt_info_t *pbpyinfo)
{
    int count = 0;
    mips32_bpt_t *bptmp;

    for (bptmp=pbpyinfo->bps; bptmp!=NULL; bptmp=bptmp->next)
    {
        if (bptmp->type == BP_TYPE_SW_BPT)
            count++;
    }

    return count;
}

static int bps_get_hwbp_count(mips32_bpt_info_t *pbpyinfo)
{
    int count = 0;
    mips32_bpt_t *bptmp;

    for (bptmp=pbpyinfo->bps; bptmp!=NULL; bptmp=bptmp->next)
    {
        if (bptmp->type == BP_TYPE_HW_BPT)
            count++;
    }

    return count;
}

static int bps_get_hwwp_count(mips32_bpt_info_t *pbpyinfo)
{
    int count = 0;
    mips32_bpt_t *bptmp;

    for (bptmp=pbpyinfo->bps; bptmp!=NULL; bptmp=bptmp->next)
    {
        if (bptmp->type == BP_TYPE_HW_WPT_W
            || bptmp->type == BP_TYPE_HW_WPT_R
            || bptmp->type == BP_TYPE_HW_WPT_RW)
            count++;
    }

    return count;
}

static inline void bps_add_to_head(mips32_bpt_info_t *pbpyinfo, mips32_bpt_t *pbp)
{
    pbp->next = pbpyinfo->bps;
    pbpyinfo->bps = pbp;
}

static mips32_bpt_t *bps_find(mips32_bpt_info_t *pbpyinfo, mips32_bpt_t *pbp)
{
    mips32_bpt_t *bptmp;

    for (bptmp=pbpyinfo->bps; bptmp!=NULL; bptmp=bptmp->next)
    {
        if (bptmp->type == pbp->type
            && bptmp->addr == pbp->addr)
            return bptmp;
    }

    return NULL;
}

int mips32_bpt_add(mips32_bpt_info_t *pbpyinfo, mips32_bpt_t *pbp)
{
    int count;
    mips32_bpt_t *bptmp;
    mips32_t *pmips32;
    mips32_cpuinfo_t *pcpuinfo;

    if (pbpyinfo == NULL || pbp == NULL)
        return -1;

    pmips32 = (mips32_t *)pbpyinfo->pmips32;
    pcpuinfo = &pmips32->cpuinfo;

    switch (pbp->type)
    {
    case BP_TYPE_HW_BPT: 
        count = bps_get_hwbp_count(pbpyinfo);
        if (count > pcpuinfo->hwbpts)
        {
            // TODO
            return -10;
        }
        break;
    case BP_TYPE_HW_WPT_W:
    case BP_TYPE_HW_WPT_R:
    case BP_TYPE_HW_WPT_RW:
        count = bps_get_hwwp_count(pbpyinfo);
        if (count > pcpuinfo->hwwpts)
        {
            // TODO
            return -10;
        }
        break;
    default:
        break;
    }

    bptmp = bps_find(pbpyinfo, pbp);
    if (bptmp == NULL)
    {
        // clone it
        bptmp = (mips32_bpt_t *)malloc(sizeof(mips32_bpt_t));
        if (bptmp == NULL)
            return -2;
        memcpy(bptmp, pbp, sizeof(mips32_bpt_t));

        // add new breakpoint to the head
        bps_add_to_head(pbpyinfo, bptmp);
        return 0;
    }

    // breakpoint exist
    bptmp->refcnt++;
    bptmp->addr_mask = pbp->addr_mask;
    bptmp->data_compare = pbp->data_compare;
    bptmp->data_compare_mask = pbp->data_compare_mask;
    bptmp->asid = pbp->asid;

    return 0;
}

int mips32_bpt_del(mips32_bpt_info_t *pbpyinfo, mips32_bpt_t *pbp)
{
    mips32_bpt_t *bptmp, *bp1;

    if (pbpyinfo == NULL || pbp == NULL || pbpyinfo->bps == NULL)
        return -1;

    if (pbpyinfo->bps->type == pbp->type
        && pbpyinfo->bps->addr == pbp->addr)
    {
        pbpyinfo->bps = pbpyinfo->bps->next;
        return NULL;
    }
    bptmp = pbpyinfo->bps;
    for (; bptmp->next!=NULL; )
    {
        if (bptmp->next->type == pbp->type
            && bptmp->next->addr == pbp->addr)
        {
            bp1 = bptmp->next;
            if (bp1->refcnt == 1)
            {
                bptmp->next = bp1->next;
                free(bp1);
            }
            else
            {
                bp1->refcnt--;
            }
            return 0;
        }
    }

    return -1;
}

int mips32_bpt_add_softbp(mips32_bpt_info_t *pbpyinfo, ubase_t addr)
{
    mips32_bpt_t softbp;

    softbp.type = BP_TYPE_SW_BPT;
    softbp.addr = addr;
    softbp.addr_mask = 0;
    
    return mips32_bpt_add(pbpyinfo, &softbp);
}

int mips32_bpt_del_softbp(mips32_bpt_info_t *pbpyinfo, ubase_t addr)
{
    mips32_bpt_t softbp;

    softbp.type = BP_TYPE_SW_BPT;
    softbp.addr = addr;
    softbp.addr_mask = 0;

    return mips32_bpt_del(pbpyinfo, &softbp);
}

int mips32_bpt_add_hardbp(mips32_bpt_info_t *pbpyinfo, ubase_t addr, ubase_t mask)
{
    mips32_bpt_t softbp;

    softbp.type = BP_TYPE_HW_BPT;
    softbp.addr = addr;
    softbp.addr_mask = mask;

    return mips32_bpt_add(pbpyinfo, &softbp);
}

int mips32_bpt_del_hardbp(mips32_bpt_info_t *pbpyinfo, ubase_t addr)
{
    mips32_bpt_t softbp;

    softbp.type = BP_TYPE_HW_BPT;
    softbp.addr = addr;
    softbp.addr_mask = 0;

    return mips32_bpt_del(pbpyinfo, &softbp);
}


static int mips32_bpt_update_softbp(mips32_t *pmips32, mips32_bpt_t *pbp, bpupdate_t active)
{
    int retv = -1;
    ubase_t instr;

    if (pbp->state == BP_STATE_SET && active == BP_ACTIVE_SET)
    {
        retv = mips32_read_word(pmips32, pbp->addr, &instr, 1);
        if (retv == 1)
        {
            pbp->instruction = instr;

            instr = MIPS32_SW_BREAKPOINT_INSTRUCTION;
            retv = mips32_write_word(pmips32, pbp->addr, &instr, 1);
            if (retv == 1)
            {
                pbp->state = BP_STATE_ACTIVE;
                // TODO, invalidate cache
            }
        }

        return retv;
    }

    if (pbp->state == BP_STATE_ACTIVE && active == BP_ACTIVE_CLR)
    {
        instr = pbp->instruction;
        retv = mips32_write_word(pmips32, pbp->addr, &instr, 1);
        if (retv == 1)
        {
            pbp->state = BP_STATE_SET;
            // TODO, invalidate cache
        }
    }
    return retv;
}

static int mips32_bpt_update_hardbp(mips32_t *pmips32, mips32_bpt_t *pbp, bpupdate_t active)
{
    return -1;
}

static int mips32_bpt_update_hardwp(mips32_t *pmips32, mips32_bpt_t *pbp, bpupdate_t active)
{
    return -1;
}

int mips32_bpt_update(mips32_bpt_info_t *pbpyinfo, mips32_bpt_t *pbpt, bpupdate_t active)
{
    int retv;
    mips32_t *pmips32;
    
    if (pbpyinfo == NULL || pbpt == NULL)
        return -1;

    pmips32 = (mips32_t *)pbpyinfo->pmips32;
    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    switch (pbpt->type)
    {
    case BP_TYPE_SW_BPT:
        retv = mips32_bpt_update_softbp(pmips32, pbpt, active);
        break;
    case BP_TYPE_HW_BPT:
        retv = mips32_bpt_update_hardbp(pmips32, pbpt, active);
        break;
    case BP_TYPE_HW_WPT_W:
    case BP_TYPE_HW_WPT_R:
    case BP_TYPE_HW_WPT_RW:
        retv = mips32_bpt_update_hardwp(pmips32, pbpt, active);
        break;
    }

    return retv;
}

int mips32_bpts_update(mips32_bpt_info_t *pbpyinfo, bpupdate_t active)
{
    int retv;
    mips32_t *pmips32;
    mips32_bpt_t *bptmp;

    if (pbpyinfo == NULL)
        return -1;

    pmips32 = (mips32_t *)pbpyinfo->pmips32;
    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    for (bptmp=pbpyinfo->bps; bptmp!=NULL; bptmp=bptmp->next)
    {
        switch (bptmp->type)
        {
        case BP_TYPE_SW_BPT:
            retv = mips32_bpt_update_softbp(pmips32, bptmp, active);
            break;
        case BP_TYPE_HW_BPT:
            retv = mips32_bpt_update_hardbp(pmips32, bptmp, active);
            break;
        case BP_TYPE_HW_WPT_W:
        case BP_TYPE_HW_WPT_R:
        case BP_TYPE_HW_WPT_RW:
            retv = mips32_bpt_update_hardwp(pmips32, bptmp, active);
            break;
        }

        if (retv < 0)
            return retv;
    }

    return 0;
}
